package org.cnc.machine;

import org.cnc.core.MachineControl.Command;

public class CncCutter {
	
	public int cutterStep = 10;
	private CutterPosition curPos;
	private CutterPosition initPos;
	private int initX = 0;
	private CutterPosition retPos;
	private Command prevCommand;

	public CncCutter () {
		
		this.curPos = new CutterPosition();
		this.initPos = this.curPos;
		this.retPos = initPos;
	}
	
	public void move ( Command to )
	{
		CutterPosition newPos = curPos;
		int step = 1;
		
		if ( to == Command.RETURN )
		{
			prevCommand = to;
			this.returnBack();
			return;
		}
		else if ( to == Command.RIGHT )
		{
			newPos.x( newPos.x() + step );
		}
		else if ( to ==  Command.LEFT )
		{
			newPos.x( newPos.x() - step );
		}
		else if ( to == Command.UP )
		{
			newPos.y( newPos.y() + step );
		}
		else if ( to == Command.DOWN )
		{
			newPos.y( newPos.y() - step );
		}
		else if ( to == Command.FORWARD )
		{
			newPos.z( newPos.z() - step );
		}
		else if ( to == Command.BACK )
		{
			newPos.z( newPos.z() + step );
		}
		else if ( to == Command.STOP )
		{
			newPos = initPos;
		}
		
		if ( prevCommand == Command.RETURN )
		{
			setRetPos( newPos );
		}
		else if ( to != Command.RIGHT && to != Command.LEFT)
		{
			setRetPos( newPos );
		}
		
		prevCommand = to;
		
		navigate( newPos );
	}
	
	private void setRetPos( CutterPosition pos )
	{
		retPos.x( initX );
		retPos.y( pos.y() );
		retPos.z( pos.z() );
		
		navigate ( retPos );
	}
	
	public void setPosition( int x, int y, int z )
	{
		System.out.println("Cutter setPos: x:"+x+" y:" + y + " z: " + z);
		
		initX = x;
		
		CutterPosition newPos = new CutterPosition();
		
		newPos.x( x );
		newPos.y( y );
		newPos.z( z );
		
		initPos = newPos;
		
		navigate(newPos);
	}
	
	public CutterPosition getPosition()
	{
		return curPos;
	}
	
	private void navigate ( CutterPosition pos )
	{
		curPos = pos;
		System.out.println("CncCore navigate: x:"+pos.x()+" y:" + pos.y() + " z: " + pos.z());
	}
	
	private void returnBack()
	{
		navigate( retPos );
	}

}
